--- /dev/null
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <io_block.h>
+#include <io_driver.h>
+#include <io_storage.h>
+#include <platform_def.h>
+#include <string.h>
+
+typedef struct {
+ io_block_dev_spec_t *dev_spec;
+ uintptr_t base;
+ size_t file_pos;
+ size_t size;
+} block_dev_state_t;
+
+#define is_power_of_2(x) ((x != 0) && ((x & (x - 1)) == 0))
+
+io_type_t device_type_block(void);
+
+static int block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity);
+static int block_seek(io_entity_t *entity, int mode, ssize_t offset);
+static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+ size_t *length_read);
+static int block_write(io_entity_t *entity, const uintptr_t buffer,
+ size_t length, size_t *length_written);
+static int block_close(io_entity_t *entity);
+static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info);
+static int block_dev_close(io_dev_info_t *dev_info);
+
+static const io_dev_connector_t block_dev_connector = {
+ .dev_open = block_dev_open
+};
+
+static const io_dev_funcs_t block_dev_funcs = {
+ .type = device_type_block,
+ .open = block_open,
+ .seek = block_seek,
+ .size = NULL,
+ .read = block_read,
+ .write = block_write,
+ .close = block_close,
+ .dev_init = NULL,
+ .dev_close = block_dev_close,
+};
+
+static block_dev_state_t state_pool[MAX_IO_BLOCK_DEVICES];
+static io_dev_info_t dev_info_pool[MAX_IO_BLOCK_DEVICES];
+
+/* Track number of allocated block state */
+static unsigned int block_dev_count;
+
+io_type_t device_type_block(void)
+{
+ return IO_TYPE_BLOCK;
+}
+
+/* Locate a block state in the pool, specified by address */
+static int find_first_block_state(const io_block_dev_spec_t *dev_spec,
+ unsigned int *index_out)
+{
+ int result = -ENOENT;
+ for (int index = 0; index < MAX_IO_BLOCK_DEVICES; ++index) {
+ /* dev_spec is used as identifier since it's unique */
+ if (state_pool[index].dev_spec == dev_spec) {
+ result = 0;
+ *index_out = index;
+ break;
+ }
+ }
+ return result;
+}
+
+/* Allocate a device info from the pool and return a pointer to it */
+static int allocate_dev_info(io_dev_info_t **dev_info)
+{
+ int result = -ENOMEM;
+ assert(dev_info != NULL);
+
+ if (block_dev_count < MAX_IO_BLOCK_DEVICES) {
+ unsigned int index = 0;
+ result = find_first_block_state(NULL, &index);
+ assert(result == 0);
+ /* initialize dev_info */
+ dev_info_pool[index].funcs = &block_dev_funcs;
+ dev_info_pool[index].info = (uintptr_t)&state_pool[index];
+ *dev_info = &dev_info_pool[index];
+ ++block_dev_count;
+ }
+
+ return result;
+}
+
+
+/* Release a device info to the pool */
+static int free_dev_info(io_dev_info_t *dev_info)
+{
+ int result;
+ unsigned int index = 0;
+ block_dev_state_t *state;
+ assert(dev_info != NULL);
+
+ state = (block_dev_state_t *)dev_info->info;
+ result = find_first_block_state(state->dev_spec, &index);
+ if (result == 0) {
+ /* free if device info is valid */
+ memset(state, 0, sizeof(block_dev_state_t));
+ memset(dev_info, 0, sizeof(io_dev_info_t));
+ --block_dev_count;
+ }
+
+ return result;
+}
+
+static int block_open(io_dev_info_t *dev_info, const uintptr_t spec,
+ io_entity_t *entity)
+{
+ block_dev_state_t *cur;
+ io_block_spec_t *region;
+
+ assert((dev_info->info != (uintptr_t)NULL) &&
+ (spec != (uintptr_t)NULL) &&
+ (entity->info == (uintptr_t)NULL));
+
+ region = (io_block_spec_t *)spec;
+ cur = (block_dev_state_t *)dev_info->info;
+ assert(((region->offset % cur->dev_spec->block_size) == 0) &&
+ ((region->length % cur->dev_spec->block_size) == 0));
+
+ cur->base = region->offset;
+ cur->size = region->length;
+ cur->file_pos = 0;
+
+ entity->info = (uintptr_t)cur;
+ return 0;
+}
+
+/* parameter offset is relative address at here */
+static int block_seek(io_entity_t *entity, int mode, ssize_t offset)
+{
+ block_dev_state_t *cur;
+
+ assert(entity->info != (uintptr_t)NULL);
+
+ cur = (block_dev_state_t *)entity->info;
+ assert((offset >= 0) && (offset < cur->size));
+
+ switch (mode) {
+ case IO_SEEK_SET:
+ cur->file_pos = offset;
+ break;
+ case IO_SEEK_CUR:
+ cur->file_pos += offset;
+ break;
+ default:
+ return -EINVAL;
+ }
+ assert(cur->file_pos < cur->size);
+ return 0;
+}
+
+static int block_read(io_entity_t *entity, uintptr_t buffer, size_t length,
+ size_t *length_read)
+{
+ block_dev_state_t *cur;
+ io_block_spec_t *buf;
+ io_block_ops_t *ops;
+ size_t aligned_length, skip, count, left, padding, block_size;
+ int lba;
+
+ assert(entity->info != (uintptr_t)NULL);
+ cur = (block_dev_state_t *)entity->info;
+ ops = &(cur->dev_spec->ops);
+ buf = &(cur->dev_spec->buffer);
+ block_size = cur->dev_spec->block_size;
+ assert((length <= cur->size) &&
+ (length > 0) &&
+ (ops->read != 0));
+
+ skip = cur->file_pos % block_size;
+ aligned_length = ((skip + length) + (block_size - 1)) &
+ ~(block_size - 1);
+ padding = aligned_length - (skip + length);
+ left = aligned_length;
+ do {
+ lba = (cur->file_pos + cur->base) / block_size;
+ if (left >= buf->length) {
+ /* Since left is larger, it's impossible to padding. */
+ if (skip) {
+ /*
+ * The beginning address (file_pos) isn't
+ * aligned with block size, we need to use
+ * block buffer to read block. Since block
+ * device is always relied on DMA operation.
+ */
+ count = ops->read(lba, buf->offset,
+ buf->length);
+ } else {
+ count = ops->read(lba, buffer, buf->length);
+ }
+ assert(count == buf->length);
+ cur->file_pos += count - skip;
+ if (skip) {
+ /*
+ * Since it's not aligned with block size,
+ * block buffer is used to store data.
+ */
+ memcpy((void *)buffer,
+ (void *)(buf->offset + skip),
+ count - skip);
+ }
+ left = left - (count - skip);
+ } else {
+ if (skip || padding) {
+ /*
+ * The beginning address (file_pos) isn't
+ * aligned with block size, we have to read
+ * full block by block buffer instead.
+ * The size isn't aligned with block size.
+ * Use block buffer to avoid overflow.
+ */
+ count = ops->read(lba, buf->offset, left);
+ } else
+ count = ops->read(lba, buffer, left);
+ assert(count == left);
+ left = left - (skip + padding);
+ cur->file_pos += left;
+ if (skip || padding) {
+ /*
+ * Since it's not aligned with block size,
+ * block buffer is used to store data.
+ */
+ memcpy((void *)buffer,
+ (void *)(buf->offset + skip),
+ left);
+ }
+ /* It's already the last block operation */
+ left = 0;
+ }
+ skip = cur->file_pos % block_size;
+ } while (left > 0);
+ *length_read = length;
+
+ return 0;
+}
+
+static int block_write(io_entity_t *entity, const uintptr_t buffer,
+ size_t length, size_t *length_written)
+{
+ block_dev_state_t *cur;
+ io_block_spec_t *buf;
+ io_block_ops_t *ops;
+ size_t aligned_length, skip, count, left, padding, block_size;
+ int lba;
+
+ assert(entity->info != (uintptr_t)NULL);
+ cur = (block_dev_state_t *)entity->info;
+ ops = &(cur->dev_spec->ops);
+ buf = &(cur->dev_spec->buffer);
+ block_size = cur->dev_spec->block_size;
+ assert((length <= cur->size) &&
+ (length > 0) &&
+ (ops->read != 0) &&
+ (ops->write != 0));
+
+ skip = cur->file_pos % block_size;
+ aligned_length = ((skip + length) + (block_size - 1)) &
+ ~(block_size - 1);
+ padding = aligned_length - (skip + length);
+ left = aligned_length;
+ do {
+ lba = (cur->file_pos + cur->base) / block_size;
+ if (left >= buf->length) {
+ /* Since left is larger, it's impossible to padding. */
+ if (skip) {
+ /*
+ * The beginning address (file_pos) isn't
+ * aligned with block size, we need to use
+ * block buffer to write block. Since block
+ * device is always relied on DMA operation.
+ */
+ count = ops->read(lba, buf->offset,
+ buf->length);
+ assert(count == buf->length);
+ memcpy((void *)(buf->offset + skip),
+ (void *)buffer,
+ count - skip);
+ count = ops->write(lba, buf->offset,
+ buf->length);
+ } else
+ count = ops->write(lba, buffer, buf->length);
+ assert(count == buf->length);
+ cur->file_pos += count - skip;
+ left = left - (count - skip);
+ } else {
+ if (skip || padding) {
+ /*
+ * The beginning address (file_pos) isn't
+ * aligned with block size, we need to avoid
+ * poluate data in the beginning. Reading and
+ * skipping the beginning is the only way.
+ * The size isn't aligned with block size.
+ * Use block buffer to avoid overflow.
+ */
+ count = ops->read(lba, buf->offset, left);
+ assert(count == left);
+ memcpy((void *)(buf->offset + skip),
+ (void *)buffer,
+ left - skip - padding);
+ count = ops->write(lba, buf->offset, left);
+ } else
+ count = ops->write(lba, buffer, left);
+ assert(count == left);
+ cur->file_pos += left - (skip + padding);
+ /* It's already the last block operation */
+ left = 0;
+ }
+ skip = cur->file_pos % block_size;
+ } while (left > 0);
+ *length_written = length;
+ return 0;
+}
+
+static int block_close(io_entity_t *entity)
+{
+ entity->info = (uintptr_t)NULL;
+ return 0;
+}
+
+static int block_dev_open(const uintptr_t dev_spec, io_dev_info_t **dev_info)
+{
+ block_dev_state_t *cur;
+ io_block_spec_t *buffer;
+ io_dev_info_t *info;
+ size_t block_size;
+ int result;
+
+ assert(dev_info != NULL);
+ result = allocate_dev_info(&info);
+ if (result)
+ return -ENOENT;
+
+ cur = (block_dev_state_t *)info->info;
+ /* dev_spec is type of io_block_dev_spec_t. */
+ cur->dev_spec = (io_block_dev_spec_t *)dev_spec;
+ buffer = &(cur->dev_spec->buffer);
+ block_size = cur->dev_spec->block_size;
+ assert((block_size > 0) &&
+ (is_power_of_2(block_size) != 0) &&
+ ((buffer->offset % block_size) == 0) &&
+ ((buffer->length % block_size) == 0));
+
+ *dev_info = info; /* cast away const */
+ (void)block_size;
+ (void)buffer;
+ return 0;
+}
+
+static int block_dev_close(io_dev_info_t *dev_info)
+{
+ return free_dev_info(dev_info);
+}
+
+/* Exported functions */
+
+/* Register the Block driver with the IO abstraction */
+int register_io_dev_block(const io_dev_connector_t **dev_con)
+{
+ int result;
+
+ assert(dev_con != NULL);
+
+ /*
+ * Since dev_info isn't really used in io_register_device, always
+ * use the same device info at here instead.
+ */
+ result = io_register_device(&dev_info_pool[0]);
+ if (result == 0)
+ *dev_con = &block_dev_connector;
+ return result;
+}
projects / project / bcm63xx / atf.git / commitdiff